#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <tf/tf.h>//角度转化的头文件
#include <geometry_msgs/Twist.h>//速度控制头文件

ros::Publisher vel_pub;//全局变量所有函数都要用

void IMUCallback(sensor_msgs::Imu msg)
{
    if(msg.orientation_covariance[0] < 0)
    {
        return;
    }
    tf::Quaternion quaternion(//转为欧拉角
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );

    double roll,pitch,yaw;
    tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);
    roll = roll*180/M_PI;
    pitch = pitch*180/M_PI;
    yaw = yaw*180/M_PI;
    ROS_INFO("翻滚 = %.0f 俯仰 = %.0f 朝向 = %.0f",roll,pitch,yaw);

    double target_yaw = 90;
    double diff_angle = target_yaw - yaw;
    geometry_msgs::Twist vel_cmd;//定义消息类型
    vel_cmd.angular.z = diff_angle * 0.01;
    vel_pub.publish(vel_cmd);


}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");//设置中文编码
    ros::init(argc, argv, "imu_node");
    
    ros::NodeHandle nh;
    ros::Subscriber imu_sub = nh.subscribe("/imu/data", 10, IMUCallback);

    vel_pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 10);//设置发布的话题及话题消息类型
    
    ros::spin();
    return 0;
}
